#include "hcpp_config.h"
#include "hcpp/ptp2_master.h"
#include "hcpp/ptp2_node_table.h"
#include "hcpp/ptp2_log.h"

#include "hal/interval_timer_hal.h"
#include "hal/slavemgm_hal.h"

static inline bool     ptp2_node_table_entry_create                (ptp2_node_table_t *this, ptp2_lut_entry_t *lut_entry_new, ptp2_lut_entry_t **lut_entry_found, uint16_t idx_search, uint16_t *idx_next);
static inline uint16_t ptp2_node_table_search_lut                  (ptp2_node_table_t *this, ptp2_node_address_t *addr, ptp2_lut_entry_t *lut_new, ptp2_lut_entry_t **lut_found);
static inline void     ptp2_node_table_remove_unused_indexes       (ptp2_node_table_t *this);

static bool     ptp2_node_table_task_lut_write              (void *ref, void *param);
static bool     ptp2_node_table_task_check_lease_durations  (void *ref, void *param);

static class_info_t class_info = {
    .size        = sizeof(ptp2_node_table_t),
    .destructor  = NULL,
    .mem_alloc   = NULL,
    .mem_free    = NULL
};

/**
 *
 */
bool
ptp2_node_table_init(ptp2_node_table_t *this)
{
    uint16_t        idx;

    object_init(this, &class_info);

    /* initialize sorted index by incrementing index */
    for (idx = 0; idx < PTP2_NODE_TABLE_MAX_ENTRIES; idx++) {
        this->idx_table[idx] = idx;
    }

    /* set size to zero */
    this->size    = 0;
    this->idx_remove_size = 0;

    /* set Er best pointer (= the best master clock for that port) to NULL */
    this->er_best = NULL;

    /* init LUT write task */
    if (!ptp2_task_init(&(this->lut_write_task), "LUT writer", ptp2_node_table_task_lut_write, this, NULL)) {
        return false;
    }
    ptp2_task_queue_add_timer(PTP2_TASK_QUEUE(), &(this->lut_write_task), true, PTP2_NODE_TABLE_LUT_WRITE_INTERVAL);

    /* init lease duration task */
    if (!ptp2_task_init(&(this->lease_durations_task), "check lease durations", ptp2_node_table_task_check_lease_durations, this, NULL)) {
        return false;
    }
    ptp2_task_queue_add_timer(PTP2_TASK_QUEUE(), &(this->lease_durations_task), true, PTP2_NODE_TABLE_CHECK_LEASE_DURATION_INTERVAL);

    return true;
}

/**
 * Get index of sorted array by comparing address with sorted list
 *
 */
void
ptp2_node_table_clear(ptp2_node_table_t *this)
{
    this->size = 0;
    slavemgm_hal_slave_table_cleanup();
    slavemgm_hal_lut_cleanup();
}

/**
 * Get index of index table by comparing address with address table
 *
 * @param   this        reference to node table
 * @param   addr        lookup this address
 * @param   lut_new     not initialised lut-entry. returns an initialised lut-entry
 * @param   lut_found   return the lut-entry if found, or NULL if not found
 * @return              array-index of index-table, if found or will be created
 */
static inline uint16_t
ptp2_node_table_search_lut(ptp2_node_table_t *this, ptp2_node_address_t *addr, ptp2_lut_entry_t *lut_entry_new, ptp2_lut_entry_t **lut_entry_found)
{
    ptp2_lut_clear_address(lut_entry_new);
    lut_entry_new->type   = addr->type;
    lut_entry_new->active = false;
    switch (addr->type) {
       case PTP2_ADDRESS_LAYER2:   lut_entry_new->mac  = addr->mac;     break;
       case PTP2_ADDRESS_IPV4:     lut_entry_new->ipv4 = addr->ipv4;    break;
       case PTP2_ADDRESS_IPV6:     lut_entry_new->ipv6 = addr->ipv6;    break;
       default:                    /* TODO: bad address? */             break;
    }

    /* binary search to find given lut entry */
    if (this->size > 0) {
        int16_t    idx_min = 0;
        int16_t    idx_max = this->size - 1;
        int16_t    idx_mid = 0;
        int16_t    idx_mid_old;
        int8_t     cmp;

        while (true) {

            /* find middle */
            idx_mid_old = idx_mid;
            idx_mid     = idx_min + ((idx_max - idx_min) / 2);

            /* didn't find slave */
            if (idx_max < idx_min) {
                (*lut_entry_found) = NULL;
                return idx_mid_old;
            }

            cmp = ptp2_lut_entry_cmp(lut_entry_new, &(this->lut_table[this->idx_table[idx_mid]]));

            /* comparator found lut entry */
            if (cmp == 0) {
                (*lut_entry_found) = &(this->lut_table[this->idx_table[idx_mid]]);
                return idx_mid;

            /* comparator found upper search field */
            } else if (cmp < 0) {
                idx_max = idx_mid - 1;

            /* comparator found lower search field */
            } else {
                idx_min = idx_mid + 1;
            }
        }
    }

    return 0;
}

/**
 * Get index of index table by comparing address with sorted list
 *
 * @param   this        reference to node table
 * @param   addr        lookup this address
 * @param   idx_found   return the index of index table
 * @return              true if found an address in slave table, false otherwise
 */
bool
ptp2_node_table_get_index_by_address(ptp2_node_table_t *this, ptp2_node_address_t *addr, uint16_t *idx_found)
{
    ptp2_lut_entry_t    lut_new;
    ptp2_lut_entry_t   *lut_found   = NULL;

    (*idx_found) = ptp2_node_table_search_lut(this, addr, &lut_new, &lut_found);

    if (lut_found) {
        return true;
    }

    return false;
}

/**
 * Create a new LUT-entry in the index-table and the lut-entry-table
 *
 * @param   this
 * @param   lut_entry_new           initialised lut-entry from lut-search
 * @param   lut_entry_found         should be NULL and returns an pointer to the created lut-entry
 * @param   idx_sort                array-index of index-table
 * @param   idx_lut                 array-index of lut-table
 * @return                          true if created a lut-entry,
 *                                  false if no more lut-entry can be managed
 */
static inline bool
ptp2_node_table_entry_create(ptp2_node_table_t *this, ptp2_lut_entry_t *lut_entry_new, ptp2_lut_entry_t **lut_entry_found, uint16_t idx_sort, uint16_t *idx_lut)
{

    if (this->size >= PTP2_NODE_TABLE_MAX_ENTRIES) {
        /* no more entries allowed */
        return false;
    }

    /* create new lut entry => point to first uninitialised lut entry */
    (*idx_lut) = this->idx_table[this->size];
    /*     size = 3, idx_sort = 1 (index), idx_lut = 2 (value), lut_entry_new = B
           idx_table[]                      lut_table[]
            ___________                              ___________
       [0] [_____3_____]                        [0] [_____C_____]
       [1] [_____0_____]                        [1] [___________]
       [2] [_____5_____]                        [2] [___________]
       [3] [_____2_____] (not initialised)      [3] [_____A_____]
           [____...____]                        [4] [___________]
                                                [5] [_____E_____]
                                                    [____...____]
    */

    /* if it's not the very first entry... fine adjust */
    if (this->size > 0) {
        /* fine adjust: compare existing lut entry with new one */
        if (ptp2_lut_entry_cmp(lut_entry_new, &(this->lut_table[this->idx_table[idx_sort]])) > 0) {
            /* if it's after the existing lut entry => increment search-index by one */
            idx_sort++;
        }

        /* shift all entries between search-index and table-size by one */
        for (uint16_t idx = this->size; idx > idx_sort; idx--) {
            this->idx_table[idx] = this->idx_table[idx - 1];
        }
        /*     size = 3, idx_sort = 1 (index), idx_lut = 2 (value), lut_entry_new = B
               idx_table[]  lut_table[]
                ___________          ___________
           [0] [_____3_____]    [0] [_____C_____]
           [1] [___________]    [1] [___________]
           [2] [_____0_____]    [2] [___________]
           [3] [_____5_____]    [3] [_____A_____]
               [____...____]    [4] [___________]
                                [5] [_____E_____]
                                    [____...____]
        */
    }

    /* insert new address-index in index-list */
    this->idx_table[idx_sort] = (*idx_lut);
    this->size++;

    /* copy values from new lut to existing lut in list */
    (*lut_entry_found)  = &(this->lut_table[(*idx_lut)]);
    (**lut_entry_found) = (*lut_entry_new); // copy by value!!

    /*     size = 4, idx_sort = 1 (index), idx_lut = 2 (value), lut_entry_new = B
           idx_table[]  lut_table[]
            ___________          ___________
       [0] [_____3_____]    [0] [_____C_____]
       [1] [_____2_____]    [1] [___________]
       [2] [_____0_____]    [2] [_____B_____]
       [3] [_____5_____]    [3] [_____A_____]
           [____...____]    [4] [___________]
                            [5] [_____E_____]
                                [____...____]
    */

    /* reset pointers to slave, address and foreign master dataset */
    (*lut_entry_found)->slave   = NULL;
    (*lut_entry_found)->address = NULL;
    (*lut_entry_found)->fm_ds   = NULL;

    return true;
}

/**
 *
 * @param   this                in      reference to a node table
 * @param   addr                in      lookup this address in the LUT table
 * @param   lut_entry_found     out
 * @param   idx_lut             a
 * @return                              returns true if LUT was found or have been created, false if no more entries are allowed.
 */
bool
ptp2_node_table_get_lut_entry_by_address(ptp2_node_table_t *this, ptp2_node_address_t *addr, ptp2_lut_entry_t **lut_entry_found, uint16_t *idx_lut)
{
    uint16_t            idx_sort;
    ptp2_lut_entry_t    lut_entry_new;

    PTP2_LOG_PRINTLN(PTP2_LOG_NODE_TABLE, PTP2_LOG_DEBUG_VERBOSE, ("this = 0x%08lx addr = 0x%08lx lut_entry_found = 0x%08lx idx_lut = 0x%08lx",
            (uintptr_t) this, (uintptr_t) addr, (uintptr_t) lut_entry_found, (uintptr_t) idx_lut));


    /* returns the array-index (not the value) of the index-table 'idx_table'
       and if found, the corresponding lut-entry 'lut_entry_found' */
    idx_sort = ptp2_node_table_search_lut(this, addr, &lut_entry_new, lut_entry_found);
    PTP2_LOG_PRINTLN(PTP2_LOG_NODE_TABLE, PTP2_LOG_DEBUG_VERBOSE, ("found lut: idx_sort = %u lut = %08lX%08lX%08lX%08lX lut_entry_found = %s", idx_sort,
                                                                                                                                    ntohl(lut_entry_new.raw[0]),
                                                                                                                                    ntohl(lut_entry_new.raw[1]),
                                                                                                                                    ntohl(lut_entry_new.raw[2]),
                                                                                                                                    ntohl(lut_entry_new.raw[3]),
                                                                                                                                    (*lut_entry_found) == NULL ? "no" : "yes"));

    /* didn't find address in lut... */
    if ((*lut_entry_found) == NULL) {
            /* returns array-index of lut-table and the lut-entry itself */
            if (!ptp2_node_table_entry_create(this, &lut_entry_new, lut_entry_found, idx_sort, idx_lut)) {
                /* no more entries allowed */
                return false;
            }
            PTP2_LOG_PRINTLN(PTP2_LOG_NODE_TABLE, PTP2_LOG_DEBUG_VERBOSE, ("created lut: idx_lut = %u lut = %08lX%08lX%08lX%08lX", *idx_lut,
                                                                                                                        ntohl(lut_entry_new.raw[0]),
                                                                                                                        ntohl(lut_entry_new.raw[1]),
                                                                                                                        ntohl(lut_entry_new.raw[2]),
                                                                                                                        ntohl(lut_entry_new.raw[3])));

    /* found address, follow up idx_lut */
    } else {
        *idx_lut = this->idx_table[idx_sort];
    }

    return true;
}


/**
 * Get slave by comparing address with sorted list.
 * If we didn't get it, we create one and sort it in.
 *
 * @param   this
 * @param   addr        lookup this address
 * @param   slave_found return looked up slave or create a new one
 * @return              true if found an address in slave table or created one,
 *                      false if no more slave can be managed
 */
bool
ptp2_node_table_get_slave_by_address(ptp2_node_table_t *this, ptp2_node_address_t *addr, ptp2_slave_t **slave_found)
{
    ptp2_lut_entry_t   *lut_entry_found = NULL;
    uint16_t            idx_lut;

    if (!ptp2_node_table_get_lut_entry_by_address(this, addr, &lut_entry_found, &idx_lut)) {
        /* no more entries allowed */
        return false;
    }

    /* no slave attached */
    if (lut_entry_found->slave == NULL) {

        /* initialize slave */
        lut_entry_found->slave   = &(this->slave_table[idx_lut]);
        lut_entry_found->address = &(this->address_table[idx_lut]);
        ptp2_slave_init(lut_entry_found->slave, lut_entry_found, lut_entry_found->address, idx_lut);
    }

    /* return corresponding slave */
    (*slave_found) = lut_entry_found->slave;

    /* update address */
    *(lut_entry_found->address) = *addr; // copy by value!!

    {
        uint16_t            idx;
        ptp2_lut_entry_t   *lut_entry;

        PTP2_LOG_PRINTLN(PTP2_LOG_NODE_TABLE, PTP2_LOG_DEBUG_VERBOSE, ("============== SLAVE TABLE ========================"));
        PTP2_LOG_PRINTLN(PTP2_LOG_NODE_TABLE, PTP2_LOG_DEBUG_VERBOSE, ("idx  lut"));
        for (idx = 0; idx < this->size; idx++) {
            idx_lut   = this->idx_table[idx];
            lut_entry = &(this->lut_table[idx_lut]);
            if (lut_entry->slave != NULL) {
                PTP2_LOG_IPV4(&(lut_entry->slave->address->ipv4), ipv4_str);
                PTP2_LOG_PRINTLN(PTP2_LOG_NODE_TABLE, PTP2_LOG_DEBUG_VERBOSE, ("%04u %04u %s", idx, idx_lut, ipv4_str));
            }
        }
        for (; idx < (this->size + 3); idx++) {
            idx_lut   = this->idx_table[idx];
            PTP2_LOG_PRINTLN(PTP2_LOG_NODE_TABLE, PTP2_LOG_DEBUG_VERBOSE, ("%04u %04u", idx, idx_lut));
        }
    }
    
    return true;
}

/**
 *
 *
 * @param   this        reference to a node table
 * @param   addr        lookup this address
 * @param   fm_ds       return looked up foreign master dataset or create a new one
 * @return              true if found an address in foreign master table or created one,
 *                      false if no more foreign master can be managed
 */
bool
ptp2_node_table_get_foreign_master_ds_by_address(ptp2_node_table_t *this, ptp2_node_address_t *addr, ptp2_foreign_master_ds_t **fm_ds)
{
    ptp2_lut_entry_t   *lut_entry_found = NULL;
    uint16_t            idx_lut;

    if (!ptp2_node_table_get_lut_entry_by_address(this, addr, &lut_entry_found, &idx_lut)) {
        /* no more entries allowed */
        return false;
    }

    /* no foreign master dataset attached */
    if (lut_entry_found->fm_ds == NULL) {

        /* initialize foreign master dataset */
        lut_entry_found->fm_ds   = &(this->foreign_master_ds[idx_lut]);
        lut_entry_found->address = &(this->address_table[idx_lut]);
        ptp2_foreign_master_ds_init(lut_entry_found->fm_ds);
    }

    /* return corresponding slave */
    (*fm_ds) = lut_entry_found->fm_ds;

    /* update address */
    *(lut_entry_found->address) = *addr; // copy by value!!

    return true;
}



static bool
ptp2_node_table_task_lut_write(void *ref, void *param)
{
    ptp2_node_table_t      *this = (ptp2_node_table_t *) ref;

    if (this->lut_changed) {
        this->lut_changed = false;
        slavemgm_hal_lut_set_lut(this->lut_table, this->idx_table, this->size);
    }

    return true;
}



/**
 * Remove unused indexes from the index table.
 * There is no need to write to the hardware. Indexes are removed only,
 * when entries have written down to hardware before.
 *
 * @param   this                    reference to node table
 */
static inline void
ptp2_node_table_remove_unused_indexes(ptp2_node_table_t *this)
{
    uint16_t idx        = 0;        /* index of this->idx_table[] / value of idx_remove_list[] */
    uint16_t value      = 0;        /* value of this->idx_table[] */
    uint16_t idx_remove = 0;        /* index of idx_remove_list[] */

    /* Iterate over index remove list, stop when index is out of range */
    /* Attention: idx_remove is always shifted by one to fulfill comparison idx_remove > 0 (unsigned!) */
    for (idx_remove = this->idx_remove_size; idx_remove > 0; idx_remove--) {
        /* backup the old value before he gets overwriten */
        idx   = this->idx_remove_list[idx_remove - 1];
        value = this->idx_table[idx];

        /* move all values upwards, from idx to (this->size - 1) */
        for (idx++; idx < this->size; idx++) {
            this->idx_table[idx - 1] = this->idx_table[idx];
        }
        /* shift the old value to the end of the used table */
        this->idx_table[idx - 1] = value;

        /* decrement size */
        this->size--;
    }

    this->idx_remove_size = 0;
}

/**
 *
 * Check slave table lease duration. Remove a LUT entry, when no slave- and foreign
 * master reference is assigned
 */
static bool
ptp2_node_table_task_check_lease_durations(void *ref, void *param)
{
    ptp2_node_table_t      *this = (ptp2_node_table_t *) ref;
    uint16_t                size_current;
    uint32_t                now  = interval_timer_hal_get_seconds();
    uint32_t                idx;
    uint32_t                idx_table;
    ptp2_lut_entry_t       *lut_entry;
    ptp2_slave_t           *slave;
    ptp2_node_address_t    *address;
    bool                    sync_announce_changed           = false;
    bool                    delay_response_changed          = false;
    bool                    global_delay_response_changed   = false;

#if PTP2_LOG
    ptp2_slave_t            copy;
#endif

    size_current = this->size;
    for (idx = 0; idx < size_current; idx++) {
        idx_table = this->idx_table[idx];
        lut_entry    = &(this->lut_table[idx_table]);
        slave       = lut_entry->slave;
        address     = lut_entry->address;

        /* no reference to other tables and lut entry is not active => mark for deletion */
        if (lut_entry->slave == NULL && lut_entry->fm_ds == NULL && lut_entry->active == false) {
            this->idx_remove_list[this->idx_remove_size++] = idx;

        /* is there a reference to a slave entry? */
        } else if (slave != NULL) {

        sync_announce_changed  = false;
        delay_response_changed = false;

#if PTP2_LOG
        // DEBUG
        copy = *slave; // copy by value!!
#endif

        /* sync was granted */
        if (slave->sync_duration > 0) {
            /* check expire time */
            if (slave->sync_expire <= now) {
                /* lease time expired => cancel grant */
                slave->sync_duration   = 0;
                slave->sync_log_period = PTP2_SLAVE_LOG_PERIOD_DENIED;

                sync_announce_changed           = true;     /* the slave entry will be written to hardware */
            }
        }

        /* announce was granted */
        if (slave->announce_duration > 0) {
            /* check expire time */
            if (slave->announce_expire <= now) {
                /* lease time expired => cancel grant */
                slave->announce_duration   = 0;
                slave->announce_log_period = PTP2_SLAVE_LOG_PERIOD_DENIED;

                sync_announce_changed           = true;     /* the slave entry will be written to hardware */
            }
        }

        /* delay response was granted */
        if (slave->delay_response_duration > 0) {
            /* check expire time */
            if (slave->delay_response_expire <= now) {
                /* lease time expired => cancel grant */
                slave->delay_response_duration   = 0;
                slave->delay_response_log_period = PTP2_SLAVE_LOG_PERIOD_DENIED;

                /* LUT entry will not be included when the whole LUT table is written to hardware */
                lut_entry->active = false;

                delay_response_changed          = true;     /* the slave entry will be written to hardware */
                global_delay_response_changed   = true;     /* the whole LUT table will be written to hardware, at the end of the function */
            }
        }

        /* if one of the flags are true, the slave entry will be written to hardware */
        if (sync_announce_changed || delay_response_changed) {

#if PTP2_LOG
          if (address->type == PTP2_ADDRESS_LAYER2) {
              PTP2_LOG_MAC(&(address->mac), mac_str);
                    PTP2_LOG_PRINTLN(PTP2_LOG_NODE_TABLE, PTP2_LOG_DEBUG,
                        ("%04lu %04lu: addr=%s sync: %lu -> %lu announce: %lu -> %lu delay_resp: %lu -> %lu", idx, idx_table, mac_str,
                   copy.sync_duration, slave->sync_duration, copy.announce_duration, slave->announce_duration, copy.delay_response_duration, slave->delay_response_duration));
          } else if (address->type == PTP2_ADDRESS_IPV4) {
              PTP2_LOG_IPV4(&(address->ipv4), ipv4_str);
                    PTP2_LOG_PRINTLN(PTP2_LOG_NODE_TABLE, PTP2_LOG_DEBUG,
                        ("%04lu %04lu: addr=%s sync: %lu -> %lu announce: %lu -> %lu delay_resp: %lu -> %lu", idx, idx_table, ipv4_str,
                   copy.sync_duration, slave->sync_duration, copy.announce_duration, slave->announce_duration, copy.delay_response_duration, slave->delay_response_duration));
          } else if (address->type == PTP2_ADDRESS_IPV6) {
              PTP2_LOG_IPV6(&(address->ipv6), ipv6_str);
                    PTP2_LOG_PRINTLN(PTP2_LOG_NODE_TABLE, PTP2_LOG_DEBUG,
                        ("%04lu %04lu: addr=%s sync: %lu -> %lu announce: %lu -> %lu delay_resp: %lu -> %lu", idx, idx_table, ipv6_str,
                   copy.sync_duration, slave->sync_duration, copy.announce_duration, slave->announce_duration, copy.delay_response_duration, slave->delay_response_duration));
          }
#endif

            /* the slave entry is written to hardware */
            slavemgm_hal_slave_table_set_entry(slave);
        }

            /* no durations are active => remove reference to slave entry */
            if (slave->sync_duration == 0 && slave->announce_duration == 0 && slave->delay_response_duration == 0) {
                lut_entry->slave = NULL;
            }
        }
    }
    /* if this flag is true, the whole LUT table is written to hardware */
    if (global_delay_response_changed) {
        PTP2_LOG_PRINTLN(PTP2_LOG_NODE_TABLE, PTP2_LOG_DEBUG_VERBOSE, ("save LUT to HW: lut_entry = 0x%08lx idx_table = 0x%08lx size = %u", (uintptr_t) this->lut_table, (uintptr_t) this->idx_table, this->size));
        slavemgm_hal_lut_set_lut(this->lut_table, this->idx_table, this->size);
    }
    PTP2_LOG_PRINTLN(PTP2_LOG_NODE_TABLE, PTP2_LOG_DEBUG_VERBOSE, ("finish check_lease_durations()"));

    ptp2_node_table_remove_unused_indexes(this);

    return true;
}


